#include "tlib.h"

static const u8 quick_table[] = {
    /*00*/ 0xff, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /*10*/ 4,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /*20*/ 5,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /*30*/ 4,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /*40*/ 6,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /*50*/ 4,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /*60*/ 5,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /*70*/ 4,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /*80*/ 7,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /*90*/ 4,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /*a0*/ 5,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /*b0*/ 4,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /*c0*/ 6,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /*d0*/ 4,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /*e0*/ 5,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
    /*f0*/ 4,    0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
};

void bitmap_init(bitmap_t *bitmap)
{
    bitmap->bitmap = 0;
}

void bitmap_set(bitmap_t *bitmap, u8 pos)
{
    bitmap->bitmap |= (1 << pos);
}
void bitmap_clear(bitmap_t *bitmap, u8 pos)
{
    bitmap->bitmap &= (~(1 << pos));
}

u32 bitmap_get_first(bitmap_t *bitmap)
{
    if (bitmap->bitmap & 0xff) {
        return quick_table[bitmap->bitmap & 0xff];
    } else if (bitmap->bitmap & 0xff00) {
        return quick_table[(bitmap->bitmap >> 8) & 0xff] + 8;
    } else if (bitmap->bitmap & 0xff0000) {
        return quick_table[(bitmap->bitmap >> 16) & 0xff] + 16;
    } else if (bitmap->bitmap & 0xff000000) {
        return quick_table[(bitmap->bitmap >> 24) & 0xff] + 24;
    }
    
    return 0xff;
}
